#include <hidef.h> /* for EnableInterrupts macro */
#include "derivative.h" /* include peripheral declarations */
#include "PAP_defs.h"
#include "APP_SMAC_API.h"
#include "freescale_radio_hardware.h"
#include "SCI_HAL_GB.h"
#include "Services.h"
#include "LCD_Handler.h"
#include "CommunicationsHandler.h"

#define CHANNEL_NUMBER 4
#define OUTPUT_POWER MAX_POWER

#pragma MESSAGE DISABLE C4000 //condition always true

UINT8 gu8Channel = CHANNEL_NUMBER;
UINT16 gu16Events = 0;

uint8 u8scheduler_counter = 0;
uint8 u8scheduler_flag = 0;
uint8 u8smac_sched_counter = 0;

uint8 gu8RTxMode; /* needed for s-mac, application can read this variable */
				  /* rtx_mode is the software state variable of the radio */

uint8 u8accel_x;
uint8 u8accel_y;
uint8 u8accel_z;
uint8 u8speed;
uint8 u8battery = 0;
uint8 u8batteryPlane = 0;
uint8 u8Signal = 0;
uint8 u8Ambientemperature = 0;
uint16 u16DistanceToAirplane = 0;
uint8 u8Directions = 0;// = DIR_POS_N | DIR_POS_E | DIR_MOVEMENT_S | DIR_MOVEMENT_E;
/*Airplane position direction respect to controller (lower part)
  Airplane movement direction (upper part)
  Codificacion for each part:		
		N  : 0x8	1000	
		NE : 0x9	1001
		NW : 0xA	1010
		
		S  : 0x4	0100
		SE : 0x5	0101
		SW : 0x6	0110
				
		 E : 0x1	0001
		 W : 0x2	0010	*/
uint8 u8PlaneSpeed=0;	//kmh
uint16 u16Altitude=0;
uint8 u8AirplaneGeneralStatus = 0;//STATUS_MOTOR_OVERHEAT | STATUS_GPS_FIX | STATUS_ULTRASONIC_ENABLED;
/*Airplane general status  ABCD EFGHb	
	All bits are 1 active
	A	A shortcircuit has been detected on main motor in the last 2 seconds
	B	GPS is ready (fix valid)
	C	Motor overheated
	D   Motor driver overheated
	E	Ultrasonic Sensor enabled
	F	HBLED enabled
	G   Accelerometer Fail
	H		*/


uint8 u8buttons;
uint8 u8MenuCounter=0; 		//Used to generate 500ms delay bases on scheduler timer
uint8 u8MenuCounterSlow=0; 		//Used to generate 2s delay bases on scheduler timer (slow blink)
uint8 u8MenuBlink=0;		//Used to blinking elements on displays
uint8 u8MenuBlinkSlow=0;		//Used to blinking elements on displays with slower frequency
uint8 u8MenuTimesCounter;	//Used to generate counts and waits based on u8MenuCounter
uint16 u16PlaneConfAnswer;	//See definitions on services.h PE PLANE_CONF_FB_GPS	
uint8 u8PlaneConfAnswerFailure; //If it is 1, it means that request cannot be processed
uint8 u8RequestPackets = 1;	//used to disable packet requests during flash programing
uint8 u8MenuStatusBK;		//Used for backup MenuStatus1 when sensor manager communication fails
uint8 u8ExtraAirplaneSettings;	
uint8 u8ControllerStatus = ~CSTATUS_MOTOR_ON;

uint8 u8sig1,u8sig2,u8sig3;
/*
	ABCD DEFGb
	All bits are 1 active
	
	A: Automatic Pilot Enabled
*/		
		

extern uint8 u8LCD_Buffer1[16];
extern uint8 u8LCD_Buffer2[16];
extern uint8 u8LCD_Buffer3[16];
extern uint8 u8LCD_Buffer4[16];

extern uint8 u8M_INIT1[16];
extern uint8 u8M_INIT2[16];
extern uint8 u8M19[16];


extern uint8 u8Comm_TxSMACPacket [COMM_TX_SMAC_PACKET_SIZE];
extern uint8 u8Comm_RxSMACPacket [COMM_RX_SMAC_PACKET_SIZE];

extern uint8 u8LCD_cursor1;
extern uint8 u8LCD_cursor2;

extern uint8 u8MenuStatus1;

uint8 u8configuration1;
uint8 u8configuration2;
uint8 u8configuration3;

extern uint8 u8Comm_CurrentSMACMsg;

extern uint16 u16Comm_CurrentMACAddr;

uint8 u8LCD_MENU = 0;

uint8 u8Comm_PacketReceived;

void main(void) 
{
    static tRxPacket rx_packet;  //SMAC structure for RX packets
	static tTxPacket tx_packet;	 //SMAC structure for TX packets
	uint16 u16smac_timeout; 
	uint8 u8tempo;
	
	//Initialize the packet.
    tx_packet.u8DataLength = COMM_TX_SMAC_PACKET_SIZE;  //Set TX default to 10
    
    //Load the address of our txbuffer into tx structure.
    tx_packet.pu8Data = &u8Comm_TxSMACPacket[0];
    	    
    rx_packet.u8DataLength = COMM_RX_SMAC_PACKET_SIZE;  //Set RX default to 20
    
    //Load the address of our rxbuffer into rx structure
    rx_packet.pu8Data = &u8Comm_RxSMACPacket[0];
    	            
    rx_packet.u8MaxDataLength = 100;	//Define the max buffer size.
    rx_packet.u8Status = 0;				//initialize the status packet to 0.

    MCUInit();		//Initialize the mcu, setup GPIOs, SPI, clock scheme, etc.
    RadioInit();
    (void)MLMESetMC13192ClockRate(0);  /* Set initial Clk speed from MC13192s CLKO*/
	UseExternalClock();	/* switch clock sources to EXTERNAL clock */
	
	/* include your start up code here */	
    EnableInterrupts; /* Turn on system interrupts */
	(void)MLMESetChannelRequest(CHANNEL_NUMBER);	
    (void)MLMEMC13192PAOutputAdjust(OUTPUT_POWER);	//Set Max power setting
    //(void)MLME  // Set radio timer prescaler for 250khz
    					 
    // Configure the transceiver in idle mode	
    (void)MLMERXDisableRequest();

    vfnServ_SetupTimeBase();	
    vfnLCD_Init(LCD_DISPLAY_BOTH);	//shows intro and puts waiting for sensor manager message
    
    vfnSCI_SetBaudRate (SCI_38400BAUD);
    vfnSCI_Init ();
    
    vfnServ_RandGenConfigure();
    
    vfnServ_SchedulerSetup();
       
  /*  vfnLCD_putBuffer1(u8M19);
    u8LCD_Buffer1[1] = 'R';
    u8LCD_Buffer1[2] = 'E';
    u8LCD_Buffer1[3] = 'S';
    u8LCD_Buffer1[4] = 'E';
    u8LCD_Buffer1[5] = 'T';
    u8LCD_Buffer1[7] = '0';
    u8LCD_Buffer1[8] = 'x';
    
    if( (SRS & 0x0F) > 9 )
    {
    	u8LCD_Buffer1[10] = (SRS & 0x0F) + ('A' - 1);    
    }
    else
    {
    	u8LCD_Buffer1[10] = (SRS & 0x0F) + '0';    
    }
    
    if( ( (SRS>>4) & 0x0F) > 9 )
    {
    	u8LCD_Buffer1[9] = ((SRS>>4) & 0x0F) + ('A' - 1);    
    }
    else
    {
    	u8LCD_Buffer1[9] = ((SRS>>4) & 0x0F) + '0';    
    }
    
    vfnLCD_SendLine(1, u8LCD_Buffer1);
    
    
    while(1)
    {
    	u8tempo = 10;
    	do{
    		vfnLCD_Delay(DELAY_2MS);
    	}while(--u8tempo);
    	
    	if (u8Comm_RequestSCIPacket()) //Request packet to QG8
        {
            vfnComm_ReadSCIPacket();    
        }
        if(u8buttons)
        {
        	u8buttons = 0;
        	break;
        }
    }
     */
     
    vfnLCD_InitMessage();
    
    vfnLCD_Init(LCD_DISPLAY_BOTH);	//shows intro and puts waiting for sensor manager message
    
   	vfnLCD_putBuffer1(u8M_INIT1);
	vfnLCD_putBuffer2(u8M_INIT2);
	vfnLCD_putBuffer3(u8M19);
	vfnLCD_putBuffer4(u8M19);
	vfn_LCD_MenuSelLine(1);
	
	vfnLCD_SendLine(1, u8LCD_Buffer1);
    vfnLCD_SendLine(2, u8LCD_Buffer2);
    vfnLCD_SendLine(3, u8LCD_Buffer3);
    vfnLCD_SendLine(4, u8LCD_Buffer4);
	     

	vfn_CommSayHelloSensorManager();//check sensor manager status. If sensor fails answer, will not
    								//continue execution
	
   // PTBDD_PTBDD4 = 0x1;//For debug purposes. REMOVE !!!!!!!!!!!!!!!!!!
    		
	//Loop forever
	u8scheduler_flag = 0;
	for (;;) 
	{
	    if (u8scheduler_flag == SCHEDULER_SCI_TASK)
	    {    
	        if(u8RequestPackets)
	        {
		        if (u8Comm_RequestSCIPacket()) //Request packet to QG8
		        {
		            // If transmission received succesfully then read packet contents
		            vfnComm_ReadSCIPacket();    
		            if(u8MenuStatus1 == 0xFF)	//If status is sensor communication error
					{
						u8MenuStatus1 = u8MenuStatusBK;	//restore to normal operation
					}
					vfnButtons_service();	
	            	
		        }
		        else
		        {
		        	if(u8MenuStatus1 != 0xFF)
		        	{
		        		u8MenuStatusBK = u8MenuStatus1;	
		        		u8MenuStatus1 = 0xFF; //Sensor comunication error
		        	}
		        }    
		    }
		    u8scheduler_flag = 0;    
	    }
	    if (u8scheduler_flag == SCHEDULER_LCD_TASK)
	    {
	    	vfnLCD_SendLine(1, u8LCD_Buffer1);
		    vfnLCD_SendLine(2, u8LCD_Buffer2);
		    vfnLCD_SendLine(3, u8LCD_Buffer3);
		    vfnLCD_SendLine(4, u8LCD_Buffer4);
		    
		    u8scheduler_flag = 0;    
	    }
	    if (u8scheduler_flag == SCHEDULER_MENU_TASK)
	    {
	        if(++u8MenuCounter == 32/4)     //Timer aprox. 500ms
		    {
		        u8MenuCounter=0;
		        vfnMenuCounterService();    //Manages special states of menu such waits and timeouts
		        u8MenuBlink = (u8MenuBlink ^ 0x01) & 0x01;			//Blink generation for menus
		        if(u8MenuCounterSlow == 3)
		        {
		        	u8MenuBlinkSlow = (u8MenuBlinkSlow ^ 0x01) & 0x01;			//Blink generation for menus
		        	u8MenuCounterSlow = 0;
		        }
		        else
		        {
		        	u8MenuCounterSlow++;
		        }
		        
		    }
	        	        
	    	vfnMenu_service();  //Menu and LCD service routine. If menu is active, LCD update will
    	                        //  alternate with menu sevice
	        u8scheduler_flag = 0;
	    }
	    if (u8scheduler_flag == SCHEDULER_SMAC_TASK)
	    {   
	        vfnComm_BuildSMACPacket ();
	    
	        (void)MLMERXDisableRequest();
	        
	        // If the channel is not free, then load a random value
	        // into the smac scheduler counter and stop the task
	        // This is done to perform the CSMA-CA algorithm
	        if (MLMEEnergyDetect() < 150)
	        {
	            u8smac_sched_counter = 1;
	            vnfServ_ChangeSchedulerPeriod();
	        }
	        else
	        {
    	        if(MCPSDataRequest(&tx_packet))
    	        {
    	        	u8configuration1 = 0;	//Delete this values if transmission was succesful
    	        	u8configuration2 = 0;
    	        	u8configuration3 = 0;
    	        }
    	        u8Comm_CurrentSMACMsg++;
	        
	        	
	        	u16smac_timeout = u16TPM_GetTimerCounter(SCHEDULER_TIMER);
	        	        
	            (void)MLMERXEnableRequest(&rx_packet, 3600 - (u16smac_timeout<<1));
	        
	        	while ((u8Comm_PacketReceived != 0x01) && (u8Comm_PacketReceived != 0xFF))
	        	{
	        		// If we succesfully received a 
	        		if(u8Comm_PacketReceived == 0xAA)
	        		{
	        			u8Comm_PacketReceived = 0;
	        			u16smac_timeout = u16TPM_GetTimerCounter(SCHEDULER_TIMER);
	        			if(u16smac_timeout < 1750)
	        			{
	        				(void)MLMERXEnableRequest(&rx_packet, (3600 - (u16smac_timeout<<1)));
	        			}
	        			else
	        			{
	        				u8Comm_PacketReceived = 0xFF;
	        			}
	        		}
	        	}
	        		        	
	        	if (u8Comm_PacketReceived == 0x01)
	        	{
	        		u8tempo =(uint8)(((uint16)(255 - MLMELinkQuality()) * 100)/255);	
	        		u8Signal = (uint8)(( (uint16)u8tempo + (uint16)u8sig1 + (uint16)u8sig2 + (uint16)u8sig3 )>>2);
	        	}
	        	else
	        	{
	        		// Packet timed out, plane did not answer.
	        		u8tempo = 0;
	        		u8Signal = 0;
	        	}
	        	
	        	u8sig3 = u8sig2;
	        	u8sig2 = u8sig1;
	        	u8sig1 = u8tempo;
	        	
	        	u8Comm_PacketReceived = 0;

	        }
	        
	        u8scheduler_flag = 0;
	    }
	    
	    
	    
	    asm
	    {
	        wait
	    }
            

	}
  /* please make sure that you never leave main */
}

uint8 MCPSDataIndication(tRxPacket *gsRxPacket)
{
    if (gsRxPacket->u8Status == SUCCESS)
    {
        if((gsRxPacket->pu8Data[0] == (uint8)(u16Comm_CurrentMACAddr >> 8)) &&
		   (gsRxPacket->pu8Data[1] == (uint8)(u16Comm_CurrentMACAddr & 0x00FF)))
    	{
    		// Read SMAC Packet here
    	    
    	    vfnComm_ReadSMACPacket();
    	    
    	    u8Comm_PacketReceived = 0x01;    	
    		
    	}
    	else   // If we succesfully receive a packet intended for another MAC address
		{
			u8Comm_PacketReceived = 0xAA;
		}

        return SUCCESS;
    }
    if (gsRxPacket->u8Status == TIMEOUT)
    {
    	/* If the plane did not respond, load the status variable with an 
         * error value 														*/
    
    	u8Comm_PacketReceived = 0xFF;
        return TIMEOUT;
    }
}

void MLMEMC13192ResetIndication() {
	//Notifies you that the MC13192 has been reset.
	//Application must handle this here.

    return;
}
